#include "main.h"
#include "tim.h"
#include "Encoder.h"

Encoder_State_t encoder_state[4] = {0}; //?????????????


// ??????????????
void Encoders_Init_ALL(void)
{
    TIM_HandleTypeDef* timers[4] = {&htim1, &htim5, &htim3, &htim4};
    
    for(int i = 0; i < 4; i++) {
        encoder_state[i].htim = timers[i];
        encoder_state[i].last_count = 0;
        encoder_state[i].current_count = (int16_t)__HAL_TIM_GET_COUNTER(timers[i]);
        encoder_state[i].speed_rpm = 0.0f;
        encoder_state[i].last_update_time = HAL_GetTick();
        
        // ????????????
        HAL_TIM_Encoder_Start(timers[i], TIM_CHANNEL_ALL);
    }
}

// ????????????????
int16_t Get_Encoder(int wheel_index)
{
    if(wheel_index < 0 || wheel_index > 3) return 0;
    return encoder_state[wheel_index].current_count;
}
